/*
   FieldVis - A Tool for Visualizing Astrophysical Magnetohydrodynamic Flow
   
   Copyright (C) 2006 - 2008     FieldVis Development Team
  
   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2 of the License, or
   (at your option) any later version.
  
   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.
  
   You should have received a copy of the GNU General Public License along
   with this program; if not, write to the Free Software Foundation, Inc.,
   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

   File     : Integrator.cpp
   Author   : Blayne Field
 
   Purpose  : Implementation file for the Integrator class.  This class
              provides methods for calculating streamline paths by
              using a 4-th order, adaptive step-size Runge-Kutta algorithm.
*/

#include "Integrator.h"

#include <list>


Point Integrator::TakeRK4Step(Point p0, float stepSize, bool *successful)
{
   // variables used in the Runge-Kutta method
   Vector3D k[4];
   Point curPosition = p0;

   // the amount of k[i] to add to p0
   float weights[4] = {0.5f, 0.5f, 1.0f, 0.0f};

   for (int i = 0; i < 4; i++)
   {
      if (!vf->InBounds(curPosition))
      {
         *successful = false;     
         return p0;
      }

      k[i] = vf->GetInterpolatedVector(curPosition) * stepSize;
      curPosition = p0 + k[i] * weights[i];
   }    

   *successful = true;
   
   return p0 + ((k[0] + k[3]) / 6.0f + (k[1] + k[2]) / 3.0f);
}

StreamlinePath Integrator::CalculateBidirectionalPath(Point p0, int maxSteps, float threshold)
{
   StreamlinePath forward = CalculatePath(p0, maxSteps, 1, threshold);
   StreamlinePath backward = CalculatePath(p0, maxSteps, -1, threshold);

   return StreamlinePath(forward, backward);
}

StreamlinePath Integrator::CalculatePath(Point p0, int maxSteps, int direction, float threshold)
{
   // figure out how much to calculate
   float minDist = 0.001;  
   float tol = 0.0001;
   float minError = tol / 10;
   float stepSize = 0.1;

   BoundVector3D  bv;
   std::list<BoundVector3D>   locs;
   std::list<BoundVector3D>::iterator  locsIter;

   // set up the initial parts of the streamline
   bv.base = p0;
   locs.push_back(bv);
   locsIter = locs.begin();

   // advance through the vector field until it exits the grid, an error occurs,
   // or the max number of points is reached
   bool successful = true;
   int pointsCalculated = 1;
   Point fullStep;
   Point halfStep1;
   Point halfStep2;
   Point currentPoint = p0;
   Point previousPoint = p0;
   float distanceFromLast = 0;

   while (pointsCalculated < maxSteps)
   {
      // if the field is too small, stop integrating
      if (vf->GetInterpolatedVector(currentPoint).Norm() <= threshold)
         break;

      // calculate the next point by using a full step,
      // and by using 2 half steps
      // stop integrating if the grid is exited
      fullStep = TakeRK4Step(currentPoint, direction * stepSize, &successful);
      if (successful)
      {
         halfStep1 = TakeRK4Step(currentPoint, direction * stepSize / 2, &successful);
         if (successful)
            halfStep2 = TakeRK4Step(halfStep1, direction * stepSize / 2, &successful);
         else
            break;
      }
      else
         break;

      // estimate the error, and adjust the step size
      float error = Point::DistanceBetween(fullStep, halfStep2);
      if (error < minError)
         error = minError;
      stepSize *= (float) pow((double) tol / error, (double) 0.2);

      // use the two results to estimate the next point
      currentPoint.SetX((16 * fullStep.GetX() - halfStep2.GetX()) / 15);
      currentPoint.SetY((16 * fullStep.GetY() - halfStep2.GetY()) / 15);
      currentPoint.SetZ((16 * fullStep.GetZ() - halfStep2.GetZ()) / 15);

      // find out how far the step went       
      distanceFromLast += Point::DistanceBetween(currentPoint, previousPoint);
      previousPoint = currentPoint;

      // update the streamline coordinates      
      if (distanceFromLast > minDist)
      {
         distanceFromLast = 0;
         pointsCalculated++;
      
         bv.base = currentPoint;
         // double assignment to make sure the directions for the last 2 points are the same
         locsIter->direction = bv.direction = Point::VectorFromTo(locsIter->base, currentPoint);
         locs.push_back(bv);
         
         locsIter++;
      }
   }

   return StreamlinePath(locs, pointsCalculated != maxSteps);
}

